(in-package #:org.shirakumo.fraf.trial)

(defstruct (contact (:include hit))
  (to-world (mat3) :type mat3)
  (velocity (vec3 0 0 0) :type vec3)
  (desired-delta 0.0 :type single-float)
  (a-inverse-mass 0.0 :type single-float)
  (b-inverse-mass 0.0 :type single-float)
  (a-relative (vec3 0 0 0) :type vec3)
  (b-relative (vec3 0 0 0) :type vec3)
  (a-rotation-change (vec3 0 0 0) :type vec3)
  (b-rotation-change (vec3 0 0 0) :type vec3)
  (a-velocity-change (vec3 0 0 0) :type vec3)
  (b-velocity-change (vec3 0 0 0) :type vec3))

(defun cancel-contact (contact)
  (setf (contact-depth contact) 0.0)
  (setf (contact-desired-delta contact) 0.0)
  (v<- (contact-a-rotation-change contact) 0)
  (v<- (contact-a-velocity-change contact) 0)
  (v<- (contact-b-rotation-change contact) 0)
  (v<- (contact-b-velocity-change contact) 0))

(defun hit-basis (hit &optional (basis (mat3)))
  (declare (optimize speed (safety 1)))
  (declare (type hit hit))
  (let ((normal (hit-normal hit))
        (tangent-0 (vec3 0 0 0))
        (tangent-1 (vec3 0 0 0)))
    (declare (dynamic-extent tangent-0 tangent-1))
    (declare (type mat3 basis))
    (cond ((< (abs (vy normal)) (abs (vx normal)))
           (let ((s (/ (sqrt (the (single-float 0.0)
                                  (+ (* (vz normal) (vz normal))
                                     (* (vx normal) (vx normal))))))))
             (vsetf tangent-0 (* (vz normal) s) 0.0 (- (* (vx normal) s)))
             (vsetf tangent-1
                    (* (vy normal) (vz tangent-0))
                    (- (* (vz normal) (vx tangent-0))
                       (* (vx normal) (vz tangent-0)))
                    (- (* (vy normal) (vx tangent-0))))))
          (T
           (let ((s (/ (sqrt (the (single-float 0.0)
                                  (+ (* (vz normal) (vz normal))
                                     (* (vy normal) (vy normal))))))))
             (vsetf tangent-0 0.0 (- (* (vz normal) s)) (* (vy normal) s))
             (vsetf tangent-1
                    (- (* (vy normal) (vz tangent-0))
                       (* (vz normal) (vy tangent-0)))
                    (- (* (vx normal) (vz tangent-0)))
                    (* (vx normal) (vy tangent-0))))))
    (with-fast-matref (m basis)
      (setf (m 0 0) (vx normal))
      (setf (m 0 1) (vx tangent-0))
      (setf (m 0 2) (vx tangent-1))
      (setf (m 1 0) (vy normal))
      (setf (m 1 1) (vy tangent-0))
      (setf (m 1 2) (vy tangent-1))
      (setf (m 2 0) (vz normal))
      (setf (m 2 1) (vz tangent-0))
      (setf (m 2 2) (vz tangent-1))
      basis)))

(defun local-velocity (to-world entity loc dt target)
  (declare (optimize speed (safety 1)))
  (declare (type vec3 loc target))
  (declare (type single-float dt))
  (let* ((vel (ntransform-inverse
               (nv+ (!vc target (the vec3 (rotation entity)) loc)
                    (the vec3 (velocity entity)))
               to-world))
         (tmp (the vec3 (last-frame-acceleration entity)))
         (acc (ntransform-inverse (nv* tmp dt) to-world)))
    (declare (dynamic-extent tmp))
    (setf (vx acc) 0f0)
    (nv+ vel acc)))

(defun desired-delta-velocity (hit velocity dt)
  (declare (optimize speed (safety 1)))
  (declare (type hit hit))
  (declare (type vec3 velocity))
  (declare (type single-float dt))
  (flet ((acc (entity)
           (let ((tmp (vec3)))
             (declare (dynamic-extent tmp))
             (v. (!v* tmp (the vec3 (last-frame-acceleration entity)) dt) (hit-normal hit)))))
    (let ((vel-from-acc (- (acc (hit-a hit)) (acc (hit-b hit))))
          (restitution (hit-restitution hit))
          (vx (vx velocity)))
      (when (< (abs vx) 0.25) ; Some kinda velocity limit magic number?
        (setf restitution 0.0))
      (+ (- vx) (* (- restitution) (- vx vel-from-acc))))))

(defun upgrade-hit-to-contact (hit dt)
  (declare (optimize speed (safety 1)))
  (declare (type contact hit))
  (declare (type single-float dt))
  (let* ((to-world (hit-basis hit (contact-to-world hit)))
         (a-relative (!v- (contact-a-relative hit) (hit-location hit) (the vec3 (location (hit-a hit)))))
         (b-relative (!v- (contact-b-relative hit) (hit-location hit) (the vec3 (location (hit-b hit)))))
         (a-velocity (vec3)) (b-velocity (vec3)))
    (declare (dynamic-extent a-velocity b-velocity))
    (local-velocity to-world (hit-a hit) a-relative dt a-velocity)
    (local-velocity to-world (hit-b hit) b-relative dt b-velocity)
    (!v- (contact-velocity hit) a-velocity b-velocity)
    (setf (contact-desired-delta hit) (desired-delta-velocity hit (contact-velocity hit) dt))
    (setf (contact-a-inverse-mass hit) (the single-float (inverse-mass (hit-a hit))))
    (setf (contact-b-inverse-mass hit) (the single-float (inverse-mass (hit-b hit))))
    (vsetf (contact-a-rotation-change hit) 0 0 0)
    (vsetf (contact-a-velocity-change hit) 0 0 0)
    (vsetf (contact-b-rotation-change hit) 0 0 0)
    (vsetf (contact-b-velocity-change hit) 0 0 0)
    hit))

(defun match-awake-state (contact)
  (declare (type contact contact))
  (let ((a (contact-a contact))
        (b (contact-b contact)))
    (when (and (/= 0 (inverse-mass a))
               (/= 0 (inverse-mass b))
               (xor (awake-p a) (awake-p b)))
      (if (awake-p a)
          (setf (awake-p b) T)
          (setf (awake-p a) T)))))

(declaim (ftype (function (contact vec3) (values vec3 &optional)) frictionless-impulse frictionful-impulse))
(defun frictionless-impulse (contact impulse)
  (declare (optimize speed (safety 1)))
  (flet ((body-delta-vel (loc body inverse-mass)
           (let ((delta-vel (vec3)))
             (declare (dynamic-extent delta-vel))
             (!vc delta-vel loc (contact-normal contact))
             (n*m (the mat3 (world-inverse-inertia-tensor body)) delta-vel)
             (!vc delta-vel delta-vel loc)
             (+ inverse-mass (v. delta-vel (contact-normal contact))))))
    (vsetf impulse
           (/ (contact-desired-delta contact)
              (+ (body-delta-vel (contact-a-relative contact)
                                 (contact-a contact)
                                 (contact-a-inverse-mass contact))
                 (body-delta-vel (contact-b-relative contact)
                                 (contact-b contact)
                                 (contact-b-inverse-mass contact))))
           0.0
           0.0)))

(defun frictionful-impulse (contact impulse)
  (declare (optimize speed (safety 1)))
  (flet ((delta-vel (delta-velocity loc inverse-inertia-tensor)
           (declare (type mat3 inverse-inertia-tensor))
           (let* ((impulse-to-torque (mat 0 (- (vz loc)) (vy loc)
                                          (vz loc) 0 (- (vx loc))
                                          (- (vy loc)) (vx loc) 0))
                  (delta-vel-world (mcopy impulse-to-torque)))
             (declare (dynamic-extent impulse-to-torque delta-vel-world))
             (nm* delta-vel-world inverse-inertia-tensor)
             (nm* delta-vel-world impulse-to-torque)
             (nm* delta-vel-world -1)
             (nm+ delta-velocity delta-vel-world))))
    (let ((inverse-mass (+ (contact-a-inverse-mass contact) (contact-b-inverse-mass contact)))
          (delta-velocity (mat3)))
      (declare (dynamic-extent delta-velocity))
      (declare (type mat3 delta-velocity))
      (when (< 0.0 (contact-a-inverse-mass contact))
        (delta-vel delta-velocity (contact-a-relative contact)
                   (world-inverse-inertia-tensor (contact-a contact))))
      (when (< 0.0 (contact-b-inverse-mass contact))
        (delta-vel delta-velocity (contact-b-relative contact)
                   (world-inverse-inertia-tensor (contact-b contact))))
      (nm* delta-velocity (contact-to-world contact))
      (!m* delta-velocity (mtranspose (contact-to-world contact)) delta-velocity)
      (with-fast-matref (m delta-velocity)
        (incf (m 0 0) inverse-mass)
        (incf (m 1 1) inverse-mass)
        (incf (m 2 2) inverse-mass))
      (let* ((vel-kill (vec (contact-desired-delta contact)
                            (- (vy (contact-velocity contact)))
                            (- (vz (contact-velocity contact)))))
             (impulse-matrix (mat3))
             (impulse (!m* impulse (!minv impulse-matrix delta-velocity) vel-kill))
             (planar-impulse (sqrt (the (single-float 0.0) (+ (expt (vy impulse) 2) (expt (vz impulse) 2)))))
             (friction (contact-dynamic-friction contact)))
        (declare (dynamic-extent vel-kill impulse-matrix))
        (when (< (* (vx impulse) (contact-static-friction contact)) planar-impulse)
          (setf (vy impulse) (/ (vy impulse) planar-impulse))
          (setf (vz impulse) (/ (vz impulse) planar-impulse))
          (setf (vx impulse) (/* (contact-desired-delta contact)
                                 (+ (miref delta-velocity 0)
                                    (* (miref delta-velocity 1) friction (vy impulse))
                                    (* (miref delta-velocity 2) friction (vz impulse)))))
          (setf (vy impulse) (* (vy impulse) friction (vx impulse)))
          (setf (vz impulse) (* (vz impulse) friction (vx impulse))))
        impulse))))
